//
//  ScenesFoaBinauralizer.cpp
//  ScenesPlayer
//
//  Created by Branch on 2017/5/2.
//  Copyright © 2017年 Scenes. All rights reserved.
//

#include "ScenesFoaBinauralizer.h"

#include <string.h>
#include "ScenesFoaOmnitoneConvolverData.h"


ScenesFoaBinauralizer::ScenesFoaBinauralizer()
: _pfScratchBuffer(nullptr)
, _pfScratchBufferL(nullptr)
, _pfScratchBufferR(nullptr)
, _pfOverlapL(nullptr)
, _pfOverlapR(nullptr)
, _nsamples(0)
, _nOverlapLength(0)
, _nFFTBins(0)
, _nFFTSize(0)
, _pFFT_cfg(nullptr)
, _pIFFT_cfg(nullptr)
, _pcpScratch(nullptr)
, _fFFTScaler(0)
, _nTaps(0)
, _isFirst(0)
{
    memset(_omnitoneFilters, 0, sizeof(kiss_fft_cpx*)*4);

}

ScenesFoaBinauralizer::~ScenesFoaBinauralizer(){

}

bool ScenesFoaBinauralizer::open(uint32_t nsamples){
    
    bool ret = false;
    if (1024 != nsamples) {
        return false;
    }
    _isFirst = 0;
    _nsamples = nsamples;

    
    
    _nTaps = sizeof(OmnitoneConvolverData_Buff) / 2 / 4;

    
    //What will the overlap size be?
    _nOverlapLength = _nsamples < _nTaps ? _nsamples - 1 : _nTaps - 1;
    //How large does the FFT need to be
    _nFFTSize = 1;
    while(_nFFTSize < (_nsamples + _nTaps + _nOverlapLength))
        _nFFTSize <<= 1;
    //How many bins is that
    _nFFTBins = _nFFTSize / 2 + 1;
    //What do we need to scale the result of the iFFT by
    _fFFTScaler = 1.f / _nFFTSize;
    
    //Deallocate any buffers with previous settings
    
    allocateBuffers();
    
    
    ret = true;
    return ret;
}

void ScenesFoaBinauralizer::close(){
    deallocateBuffers();
}

bool ScenesFoaBinauralizer::processAudio(float** src,float** dest){
    
    uint32_t niChannel = 0;
    uint32_t ni = 0;
    kiss_fft_cpx cpTemp;

    
    memset(_pfScratchBufferL, 0, _nFFTSize * sizeof(float));
    memset(_pfScratchBufferR, 0, _nFFTSize * sizeof(float));
    
    for(niChannel = 0; niChannel < 4; niChannel++)
    {
        memcpy(_pfScratchBuffer, src[niChannel], _nsamples * sizeof(float));
        
        
        memset(&_pfScratchBuffer[_nsamples], 0, (_nFFTSize - _nsamples) * sizeof(float));
        kiss_fftr(_pFFT_cfg, _pfScratchBuffer, _pcpScratch);
        for(ni = 0; ni < _nFFTBins; ni++)
        {
            cpTemp.r = _pcpScratch[ni].r * _omnitoneFilters[niChannel][ni].r
            - _pcpScratch[ni].i * _omnitoneFilters[niChannel][ni].i;
            cpTemp.i = _pcpScratch[ni].r * _omnitoneFilters[niChannel][ni].i
            + _pcpScratch[ni].i * _omnitoneFilters[niChannel][ni].r;
            _pcpScratch[ni] = cpTemp;
        }
        kiss_fftri(_pIFFT_cfg, _pcpScratch, _pfScratchBuffer);
        
        //左声道
        for(ni = 0; ni < _nFFTSize; ni++)
            _pfScratchBufferL[ni] += _pfScratchBuffer[ni];
        //右声道,
        if(1 == niChannel){
            //Y通道需要取反
            for(ni = 0; ni < _nFFTSize; ni++)
                _pfScratchBufferR[ni] -= _pfScratchBuffer[ni];
        }else{
            for(ni = 0; ni < _nFFTSize; ni++)
                _pfScratchBufferR[ni] += _pfScratchBuffer[ni];
        }
        
    }
    
    for(ni = 0; ni < _nFFTSize; ni++){
        _pfScratchBufferL[ni] *= _fFFTScaler;
        _pfScratchBufferR[ni] *= _fFFTScaler;
    }
    
    
    //左声道尾部
    memcpy(dest[0], _pfScratchBufferL, _nsamples * sizeof(float));
    for(ni = 0; ni < _nOverlapLength; ni++){
        dest[0][ni] += _pfOverlapL[ni];
    }
    memcpy(_pfOverlapL, &_pfScratchBufferL[_nsamples], _nOverlapLength * sizeof(float));
    
    //右声道尾部
    memcpy(dest[1], _pfScratchBufferR, _nsamples * sizeof(float));
    for(ni = 0; ni < _nOverlapLength; ni++){
        dest[1][ni] += _pfOverlapR[ni];
    }
    memcpy(_pfOverlapR, &_pfScratchBufferR[_nsamples], _nOverlapLength * sizeof(float));
    
    
    if(_isFirst < 5){
        memset(dest[0], 0, _nsamples * sizeof(float));
        memset(dest[1], 0, _nsamples * sizeof(float));
        _isFirst++;
    }
    return true;
}

void ScenesFoaBinauralizer::allocateBuffers(){
    //Allocate scratch buffers
    _pfScratchBuffer = new float[_nFFTSize];
    _pfScratchBufferL = new float[_nFFTSize];
    _pfScratchBufferR = new float[_nFFTSize];
    //Allocate overlap-add buffers
    _pfOverlapL = new float[_nOverlapLength];
    _pfOverlapR = new float[_nOverlapLength];
    memset(_pfOverlapL, 0, _nOverlapLength*sizeof(float));
    memset(_pfOverlapR, 0, _nOverlapLength*sizeof(float));
    
    //Allocate FFT and iFFT for new size
    _pFFT_cfg = kiss_fftr_alloc(_nFFTSize, 0, 0, 0);
    _pIFFT_cfg = kiss_fftr_alloc(_nFFTSize, 1, 0, 0);
    
    //Allocate the FFTBins for each channel, for each ear

    
    _pcpScratch = new kiss_fft_cpx[_nFFTBins];
    
    
    for (uint32_t i = 0; i < 4; ++i) {
        _omnitoneFilters[i] = new kiss_fft_cpx[_nFFTBins];
    }
    initOmnitoneFilters();
}

void ScenesFoaBinauralizer::deallocateBuffers(){
    if(_pfScratchBuffer){
        delete [] _pfScratchBuffer;
        _pfScratchBuffer = nullptr;
    }
    if(_pfScratchBufferL){
        delete [] _pfScratchBufferL;
        _pfScratchBufferL = nullptr;
    }
    if(_pfScratchBufferR){
        delete [] _pfScratchBufferR;
        _pfScratchBufferR = nullptr;
    }
    if(_pfOverlapL){
        delete [] _pfOverlapL;
        _pfOverlapL = nullptr;
    }
    if(_pfOverlapR){
        delete [] _pfOverlapR;
        _pfOverlapR = nullptr;
    }
    if(_pFFT_cfg){
        kiss_fftr_free(_pFFT_cfg);
        _pFFT_cfg = nullptr;
    }
    if(_pIFFT_cfg){
        kiss_fftr_free(_pIFFT_cfg);
        _pIFFT_cfg = nullptr;
    }
    
    
    
    if(_pcpScratch){
        delete [] _pcpScratch;
        _pcpScratch = nullptr;
    }
    
    
    for (uint32_t i = 0; i < 4; ++i) {
        if (_omnitoneFilters[i]) {
            delete [] _omnitoneFilters[i];
            _omnitoneFilters[i] = nullptr;
        }
    }
    
    
}

void ScenesFoaBinauralizer::initOmnitoneFilters(){

    
    uint32_t samples = sizeof(OmnitoneConvolverData_Buff) / 2 / 4;
    
    
    
    float* pf[4];
    float* tempf[4];
    for (int i = 0; i < 4; ++i) {
        tempf[i] = pf[i] = new float[_nFFTSize];
        memset(pf[i], 0, _nFFTSize*sizeof(float));
    }
    
    int16_t* tempBuffS = (int16_t*)OmnitoneConvolverData_Buff;
    for (uint32_t i = 0; i < samples; i++) {
        
        for(uint32_t ni = 0; ni < 4; ni++)
        {
            *tempf[ni]++ = *tempBuffS++ / 32767.f;
        }
    }
    
    for (int i = 0; i < 4; ++i) {
        
        kiss_fftr(_pFFT_cfg, pf[i], _omnitoneFilters[i]);
    }
    

    for (int i = 0; i < 4; ++i) {
        delete[] pf[i];
    }
}







